
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       LowPassFilter2pVector3f.c
  * @author     baiyang
  * @date       2022-4-14
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "LowPassFilter2p.h"

#include <string.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void DBFVector3f_ctor(DigitalBiquadFilterVector3f *filter)
{
    filter->_delay_element_1.x = 0.0f;
    filter->_delay_element_1.y = 0.0f;
    filter->_delay_element_1.z = 0.0f;

    filter->_delay_element_2.x = 0.0f;
    filter->_delay_element_2.y = 0.0f;
    filter->_delay_element_2.z = 0.0f;

    filter->initialised        = false;
}

Vector3f_t DBFVector3f_apply(DigitalBiquadFilterVector3f *filter, const Vector3f_t* sample, const struct biquad_params *params)
{
    if(math_flt_zero(params->cutoff_freq) || math_flt_zero(params->sample_freq)) {
        return *sample;
    }

    if (!filter->initialised) {
        DBFVector3f_reset2(filter, sample, params);
        filter->initialised = true;
    }

    Vector3f_t delay_element_0;
    delay_element_0.x = sample->x - filter->_delay_element_1.x * params->a1 - filter->_delay_element_2.x * params->a2;
    delay_element_0.y = sample->y - filter->_delay_element_1.y * params->a1 - filter->_delay_element_2.y * params->a2;
    delay_element_0.z = sample->z - filter->_delay_element_1.z * params->a1 - filter->_delay_element_2.z * params->a2;

    Vector3f_t output;
    output.x = delay_element_0.x * params->b0 + filter->_delay_element_1.x * params->b1 + filter->_delay_element_2.x * params->b2;
    output.y = delay_element_0.y * params->b0 + filter->_delay_element_1.y * params->b1 + filter->_delay_element_2.y * params->b2;
    output.z = delay_element_0.z * params->b0 + filter->_delay_element_1.z * params->b1 + filter->_delay_element_2.z * params->b2;

    filter->_delay_element_2 = filter->_delay_element_1;
    filter->_delay_element_1 = delay_element_0;

    return output;
}

void DBFVector3f_reset(DigitalBiquadFilterVector3f *filter)
{
    filter->_delay_element_1.x = 0.0f;
    filter->_delay_element_1.y = 0.0f;
    filter->_delay_element_1.z = 0.0f;

    filter->_delay_element_2.x = 0.0f;
    filter->_delay_element_2.y = 0.0f;
    filter->_delay_element_2.z = 0.0f;

    filter->initialised = false;
}
void DBFVector3f_reset2(DigitalBiquadFilterVector3f *filter, const Vector3f_t* value, const struct biquad_params *params)
{
    filter->_delay_element_1.x = filter->_delay_element_2.x = value->x * (1.0f / (1 + params->a1 + params->a2));
    filter->_delay_element_1.y = filter->_delay_element_2.y = value->y * (1.0f / (1 + params->a1 + params->a2));
    filter->_delay_element_1.z = filter->_delay_element_2.z = value->z * (1.0f / (1 + params->a1 + params->a2));

    filter->initialised = true;
}

/**
  * @brief       
  * @param[in]   lpf_2p_vector3  
  * @param[out]  
  * @retval      
  * @note        
  */
void LPF2pVector3f_ctor(LowPassFilter2pVector3f* lpf_2p_vector3)
{
    DBFVector3f_ctor(&lpf_2p_vector3->_filter);
    memset(&lpf_2p_vector3->_params, 0, sizeof(lpf_2p_vector3->_params));
}

void LPF2pVector3f_ctor2(LowPassFilter2pVector3f* lpf_2p_vector3, float sample_freq, float cutoff_freq)
{
    DBFVector3f_ctor(&lpf_2p_vector3->_filter);

    // set initial parameters
    LPF2pVector3f_set_cutoff_frequency(lpf_2p_vector3, sample_freq, cutoff_freq);
}

void LPF2pVector3f_set_cutoff_frequency(LowPassFilter2pVector3f* lpf_2p_vector3, float sample_freq, float cutoff_freq)
{
    DigitalBiquadFilter_compute_params(sample_freq, cutoff_freq, &lpf_2p_vector3->_params);
}

Vector3f_t LPF2pVector3f_apply(LowPassFilter2pVector3f* lpf_2p_vector3, const Vector3f_t* sample)
{
    if (!math_flt_positive(lpf_2p_vector3->_params.cutoff_freq)) {
        // zero cutoff means pass-thru
        return *sample;
    }

    return DBFVector3f_apply(&lpf_2p_vector3->_filter, sample, &lpf_2p_vector3->_params);
}

void LPF2pVector3f_reset(LowPassFilter2pVector3f* lpf_2p_vector3)
{
    return DBFVector3f_reset(&lpf_2p_vector3->_filter);
}

void LPF2pVector3f_reset2(LowPassFilter2pVector3f* lpf_2p_vector3, const Vector3f_t* value)
{
    return DBFVector3f_reset2(&lpf_2p_vector3->_filter, value, &lpf_2p_vector3->_params);
}
/*------------------------------------test------------------------------------*/


